#include "gencolors.hpp"

namespace autoware_ground_segmentation
{

std::vector<cv::Scalar> generateColors(size_t num_colors)
{
  std::vector<cv::Scalar> colors;
  colors.reserve(num_colors);
  
  // Generate distinct colors
  for (size_t i = 0; i < num_colors; ++i) {
    // Use HSV color space for better color distribution
    double hue = (i * 360.0) / num_colors;
    double saturation = 0.8;
    double value = 0.9;
    
    // Convert HSV to BGR (OpenCV format)
    double c = value * saturation;
    double x = c * (1 - std::abs(std::fmod(hue / 60.0, 2.0) - 1));
    double m = value - c;
    
    double r, g, b;
    if (hue < 60) {
      r = c; g = x; b = 0;
    } else if (hue < 120) {
      r = x; g = c; b = 0;
    } else if (hue < 180) {
      r = 0; g = c; b = x;
    } else if (hue < 240) {
      r = 0; g = x; b = c;
    } else if (hue < 300) {
      r = x; g = 0; b = c;
    } else {
      r = c; g = 0; b = x;
    }
    
    colors.emplace_back((b + m) * 255, (g + m) * 255, (r + m) * 255);
  }
  
  return colors;
}

} // namespace autoware_ground_segmentation 